/**
  *************************(C) COPYRIGHT 2021 LionHeart*************************
  * @file       fall_down_task.h
  * @brief      升降任务
  * @note       
  * @history
  *  Version    Date                 Author               Modification
  *  V1.0.0     2021/12/20 16:01     yanfubing            1. done
  @verbatim
  ==============================================================================
1.准备升降架
 arnning:fall_down_motor==go_down_motor
  ==============================================================================
  @endverbatim
  *************************(C) COPYRIGHT 2021 LionHeart*************************
  */   
#ifndef FALL_DOWN_TASK_H
#define FALL_DOWN_TASK_H
#include "bsp_can.h"
#include "CAN_receive.h"
#include "remote_control.h"
#include "main.h"
#include "pid.h"
#include "user_lib.h"

#define MAX_HIGH  750

#define go_check()     HAL_GPIO_ReadPin(J2_GPIO_Port,J2_Pin)
#define down_check()   HAL_GPIO_ReadPin(J1_GPIO_Port,J1_Pin)
//电机的RMP转换为角度
#define MOTOR_ECD_TO_ANGLE    1/400
//只按键的最大速度
#define fall_down_MAX_SPEED   120
//升降任务延时
#define fall_down_task_INIT_TIME 5
//夹取任务一阶滤波系数
#define engineering_CONTROL_TIME 0.002f 
/**************升降电机，速度闭环PID M3508************/
#define M3508_fall_down_ANGLE_PID_KP 50.0f
#define M3508_fall_down_ANGLE_PID_KI 0.00f
#define M3508_fall_down_ANGLE_PID_KD 0.1f
#define M3508_fall_down_ANGLE_PID_MAX_OUT 3500.0f
#define M3508_fall_down_ANGLE_PID_MAX_IOUT 5.0f

#define M3508_fall_down_SPEED_PID_KP 10.0f
#define M3508_fall_down_SPEED_PID_KI 0.00f
#define M3508_fall_down_SPEED_PID_KD 0.8f
#define M3508_fall_down_SPEED_PID_MAX_OUT 4000.0f
#define M3508_fall_down_SPEED_PID_MAX_IOUT 5.0f

#define engineering_Clamp_turn_NUM 0.1666666667f
/*********升降状态机********/
typedef enum          
{
	In=0,//升降状态机初始化 
	Act,         
	Res,            
} fall_down_state_e;

typedef enum
{
  OFF=0,
  ON =1,
}limit_e;


typedef struct
{  
	const motor_measure_t *fall_down_motor_measure;
	int16_t target_speed;
	fp32 angle;
	fp32 mileage_set;
	int16_t give_current;
} fall_down_motor_t;

typedef struct 
{               
  const RC_ctrl_t *fall_down_RC;              //遥控器的指针
	const can2_receive_t *fall_down_can_rc;
	limit_e Limit_Key_state[2];                 //上下两个限位开关
	fall_down_state_e fall_down_state;            //救援状态机
	fp32 fall_down_Init_Angle[2];                 //电机角度初始化
	fall_down_motor_t engineering_fall_down_motor_data[2];  //升降m3508电机
	pid_type_def engineering_fall_down_motor_angle_pid[2];
	pid_type_def engineering_fall_down_motor_pid[2];     
	                                           //使用一阶低通滤波减缓设定值
	first_order_filter_type_t engineering_cmd_slow_set_fall_down;   
	
	fp32 fall_down_motor_max_speed;          //升降电机最大速度     
	 
}fall_down_control_t;

extern fall_down_control_t fall_down_move;
/**
	* @brief          升降任务
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
extern void fall_down_task(void const* pvParameters);
#endif

